﻿using System;
using System.IO;
using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.Windows.Forms;

namespace CarController
{
    class CarNetworkInterface
    {
        static Pololu servoControl;
        static TcpClient UIClient, centralServer;
        static StreamReader serverRead;
        static StreamWriter serverWrite;
        static StreamReader UIReader;
        static StreamWriter UIWriter;
        static StreamReader activesr;
        static TcpListener UIlistener;
        static DateTime lastHeartbeat;
        static Display d;
        static bool listening;
        static bool centralserver = false;
        static bool direct = false;
        static int count = 0;
        public static long difference = 0;

        static string carname = "Kermit";
        static int vfov = 40;
        static int hfov = 60;
        static double camheight = 0.4;
        static double frontofcar = 0.2;
        static double turningradius = 2.0;

        [STAThread]
        static void Main(string[] args)
        {
            //create servo controller
            servoControl = new Pololu("COM1");
            Console.WriteLine("Type 0 to accept direct connections or 1 for relay only.");
            if (Console.ReadLine() == "0")
            {
                direct = true;

            }
            while (true)
            {  
                if (direct)
                {
                    directAllowed();
                }
                else
                {
                    Console.WriteLine("No direct connections - relay through central server only.");
                    relayOnly();
                }

            }
        }

        static void directAllowed()
        {
            try
            {
                //register with name server if available
                centralServer = new TcpClient();
                centralServer.Connect("127.0.0.1", 9092);
                Console.WriteLine("Connected to server.");
                serverRead = new StreamReader(centralServer.GetStream());
                serverWrite = new StreamWriter(centralServer.GetStream());

                //inform server of direct connections
                serverWrite.WriteLine("9090:" + carname + ":" + vfov + ":" + hfov + ":" + camheight + ":" + frontofcar + ":" + turningradius);
                serverWrite.WriteLine("DIRECT");
                serverWrite.Flush();
            }
            catch
            {
                Console.WriteLine("No name server...");
            }

            Console.WriteLine("Listening on port 9090 for incoming connections.");
            UIlistener = new TcpListener(IPAddress.Any, 9090);
            listening = true;
            UIlistener.Start();
            try
            {
                //wait for connection from user interface
                UIClient = UIlistener.AcceptTcpClient();
                UIlistener.Stop();
                listening = false;
                Console.WriteLine("Connected to " + UIClient.Client.RemoteEndPoint);
                UIReader = new StreamReader(UIClient.GetStream());
                UIWriter = new StreamWriter(UIClient.GetStream());

                //start monitoring ALIVE messages
                lastHeartbeat = DateTime.Now;
                new Thread(MonitorHeartbeat).Start();

                //start accepting data from UI.
                activesr = UIReader;
                new Thread(readData).Start();

                //start sending the video
                d = new Display(UIClient);
                Application.Run(d);
            }
            catch (SocketException ex)
            {
                UIClient.Close();
                Console.WriteLine(ex.StackTrace);
            }
        }

        static void relayOnly()
        {
            
            try
            {
                //Establish connection to central server on port 9092
                centralServer = new TcpClient();
                centralServer.Connect("127.0.0.1", 9092);
                Console.WriteLine("Connected to server.");
                serverRead = new StreamReader(centralServer.GetStream());
                serverWrite = new StreamWriter(centralServer.GetStream());

                //inform server of no direct connections
                serverWrite.WriteLine("9090:" + carname + ":" + vfov + ":" + hfov + ":" + camheight + ":" + frontofcar + ":" + turningradius);
                serverWrite.WriteLine("RELAY");
                serverWrite.Flush();

                //start monitoring ALIVE messages
                centralserver = true;
                lastHeartbeat = DateTime.Now;
                //new Thread(MonitorHeartbeat).Start();

                if (serverRead.ReadLine() == "INITKERMIT")
                {
                    //start reading data being sent by the server
                    activesr = serverRead;
                    new Thread(readData).Start();

                    d = new Display(centralServer);
                    Application.Run(d);
                }
                else
                {
                    Console.WriteLine("Server initial password failure. Disconnecting...");
                    centralServer.Close();
                }
            }
            catch
            {
                Console.WriteLine("Central server down, exiting...");
                Environment.Exit(0);
            }
        }

        static void readData()
        {
            try
            {
                while (!activesr.EndOfStream)
                {
                    string cmd = activesr.ReadLine();

                    if (cmd.StartsWith("ALIVE"))
                    {
                        long ticks = long.Parse(cmd.Split(':')[1]);
                        lastHeartbeat = DateTime.Now;
                        difference = lastHeartbeat.Ticks - ticks;
                    }
                    else
                    {
                            Console.WriteLine("COMMAND: " + cmd);

                            if (cmd.StartsWith("SPEED:"))
                            {
                                //range check: -1 to 1
                                double speed = Math.Max(double.Parse(cmd.Split(':')[1]), -1.0);
                                speed = Math.Min(speed, 1.0);
                                //send to servos
                                servoControl.SetSpeed(speed);
                            }
                            else if (cmd.StartsWith("STEER:"))
                            {
                                //range check: -1 to 1
                                double steer = Math.Max(double.Parse(cmd.Split(':')[1]), -1.0);
                                steer = Math.Min(steer, 1.0);
                                //send to servos
                                servoControl.SetSteering(steer);
                            }
                            else if (cmd.StartsWith("PAN:"))
                            {
                                //range check: -1 to 1
                                double pan = Math.Max(double.Parse(cmd.Split(':')[1]), -1.0);
                                pan = Math.Min(pan, 1.0);
                                //send to servos
                                servoControl.SetPan(pan);
                            }
                            else if (cmd.StartsWith("TILT:"))
                            {
                                //range check: -1 to 1
                                double tilt = Math.Max(double.Parse(cmd.Split(':')[1]), -1.0);
                                tilt = Math.Min(tilt, 1.0);
                                //send to servos
                                servoControl.SetTilt(tilt);
                            }
                            else if (cmd == "KILL")
                            {
                                servoControl.Kill();
                            }
                            else if (cmd.StartsWith("HEIGHT:"))
                            {
                                //range check: 1 to 480
                                double height = Math.Max(double.Parse(cmd.Split(':')[1]), 1.0);
                                height = Math.Min(height, 480.0);
                                //update display
                                d.imageheight = (int)height;
                            }
                            else if (cmd.StartsWith("WIDTH:"))
                            {
                                //range check: 1 to 640
                                double width = Math.Max(double.Parse(cmd.Split(':')[1]), 1.0);
                                width = Math.Min(width, 640.0);
                                //update display
                                d.imagewidth = (int)width;
                            }
                            else if (cmd.StartsWith("STEERFACTOR:"))
                            {
                                servoControl.UpdateSteerFactor( double.Parse(cmd.Split(':')[1]));
                            }
                            else if (cmd.StartsWith("TILTFACTOR:"))
                            {
                                servoControl.UpdateTiltFactor( double.Parse(cmd.Split(':')[1]));
                            }
                        }
                }
            }
            catch
            {
            }
        }

        static void MonitorHeartbeat()
        {
            Thread.CurrentThread.IsBackground = true;
            
            while (true)
            {
                if (centralserver)
                { }
                else
                {
                    if (DateTime.Now - lastHeartbeat > TimeSpan.FromSeconds(2))
                    {
                        count++;
                        servoControl.Kill();
                        if (!listening)
                        {
                            Console.Clear();
                            Console.WriteLine(DateTime.Now.ToString() + "Timeout. Killing.");
                            if (count > 10)
                            {

                                d.Close();
                                d.Dispose();
                                count = 0;
                            }
                        }
                    }
                }
                Thread.Sleep(500);

            }
        }

    }
}